#include "nml_intf/canon_position.hh"
#include <math.h>
#include "posemath.h"

static double noElement = 0.0;

CANON_POSITION::CANON_POSITION(double _x, double _y, double _z, double _a, double _b, double _c, double _u, double _v, double _w)
{
    this->x = _x;
    this->y = _y;
    this->z = _z;
    this->a = _a;
    this->b = _b;
    this->c = _c;
    this->u = _u;
    this->v = _v;
    this->w = _w;
}

CANON_POSITION::CANON_POSITION(const ZucPose& _pos)
{
    this->x = _pos.tran.x;
    this->y = _pos.tran.y;
    this->z = _pos.tran.z;
    this->a = _pos.a;
    this->b = _pos.b;
    this->c = _pos.c;
    this->u = _pos.u;
    this->v = _pos.v;
    this->w = _pos.w;
}

CANON_POSITION::CANON_POSITION(PM_CARTESIAN const& xyz) : a(0.0), b(0.0), c(0.0), u(0.0), v(0.0), w(0.0)
{
    this->x = xyz.x;
    this->y = xyz.y;
    this->z = xyz.z;
}

CANON_POSITION::CANON_POSITION(PM_CARTESIAN const& xyz, PM_CARTESIAN const& abc) : u(0.0), v(0.0), w(0.0)
{
    this->x = xyz.x;
    this->y = xyz.y;
    this->z = xyz.z;
    this->a = abc.x;
    this->b = abc.y;
    this->c = abc.z;
}

bool CANON_POSITION::operator==(const CANON_POSITION& o) const
{
    return (this->x == o.x && this->y == o.y && this->z == o.z && this->a == o.a && this->b == o.b && this->c == o.c && this->u == o.u && this->v == o.v &&
            this->w == o.w);
}
bool CANON_POSITION::operator!=(const CANON_POSITION& o) const
{
    return (this->x != o.x || this->y != o.y || this->z != o.z || this->a != o.a || this->b != o.b || this->c != o.c || this->u != o.u || this->v != o.v ||
            this->w != o.w);
}
CANON_POSITION& CANON_POSITION::operator+=(const CANON_POSITION& o)
{
    this->x += o.x;
    this->y += o.y;
    this->z += o.z;
    this->a += o.a;
    this->b += o.b;
    this->c += o.c;
    this->u += o.u;
    this->v += o.v;
    this->w += o.w;
    return *this;
}
CANON_POSITION& CANON_POSITION::operator+=(const ZucPose& o)
{
    this->x += o.tran.x;
    this->y += o.tran.y;
    this->z += o.tran.z;
    this->a += o.a;
    this->b += o.b;
    this->c += o.c;
    this->u += o.u;
    this->v += o.v;
    this->w += o.w;
    return *this;
}

const CANON_POSITION CANON_POSITION::operator+(const CANON_POSITION& o) const
{
    CANON_POSITION result = *this;
    result += o;
    return result;
}

const CANON_POSITION CANON_POSITION::operator+(const ZucPose& o) const
{
    CANON_POSITION result = *this;
    result += o;
    return result;
}
CANON_POSITION& CANON_POSITION::operator-=(const CANON_POSITION& o)
{
    this->x -= o.x;
    this->y -= o.y;
    this->z -= o.z;
    this->a -= o.a;
    this->b -= o.b;
    this->c -= o.c;
    this->u -= o.u;
    this->v -= o.v;
    this->w -= o.w;
    return *this;
}

CANON_POSITION& CANON_POSITION::operator-=(const ZucPose& o)
{
    this->x -= o.tran.x;
    this->y -= o.tran.y;
    this->z -= o.tran.z;
    this->a -= o.a;
    this->b -= o.b;
    this->c -= o.c;
    this->u -= o.u;
    this->v -= o.v;
    this->w -= o.w;
    return *this;
}

const CANON_POSITION CANON_POSITION::operator-(const CANON_POSITION& o) const
{
    CANON_POSITION result = *this;
    result -= o;
    return result;
}
const CANON_POSITION CANON_POSITION::operator-(const ZucPose& o) const
{
    CANON_POSITION result = *this;
    result -= o;
    return result;
}

const double CANON_POSITION::max() const
{
    double res = x;
    res = fmax(res, this->y);
    res = fmax(res, this->z);
    res = fmax(res, this->a);
    res = fmax(res, this->b);
    res = fmax(res, this->c);
    res = fmax(res, this->u);
    res = fmax(res, this->v);
    res = fmax(res, this->w);
    return res;
}

const CANON_POSITION CANON_POSITION::abs() const
{
    CANON_POSITION result;
    result.x = fabs(this->x);
    result.y = fabs(this->y);
    result.z = fabs(this->z);
    result.a = fabs(this->a);
    result.b = fabs(this->b);
    result.c = fabs(this->c);
    result.u = fabs(this->u);
    result.v = fabs(this->v);
    result.w = fabs(this->w);
    return result;
}

const CANON_POSITION CANON_POSITION::absdiff(const CANON_POSITION& o) const
{
    CANON_POSITION result = *this;
    result -= o;
    return result.abs();
}

const ZucPose CANON_POSITION::toZucPose() const
{
    ZucPose out;
    out.tran.x = this->x;
    out.tran.y = this->y;
    out.tran.z = this->z;
    out.a = this->a;
    out.b = this->b;
    out.c = this->c;
    out.u = this->u;
    out.v = this->v;
    out.w = this->w;
    return out;
}

const PM_CARTESIAN CANON_POSITION::xyz() const { return PM_CARTESIAN(this->x, this->y, this->z); }

void CANON_POSITION::set_xyz(const PM_CARTESIAN& xyz)
{
    this->x = xyz.x;
    this->y = xyz.y;
    this->z = xyz.z;
}

const PM_CARTESIAN CANON_POSITION::abc() const { return PM_CARTESIAN(this->a, this->b, this->c); }

const PM_CARTESIAN CANON_POSITION::uvw() const { return PM_CARTESIAN(this->u, this->v, this->w); }

void CANON_POSITION::print() const
{
    printf("x %g y %g z %g a %g b %g c %g u %g v %g w %g ", this->x, this->y, this->z, this->a, this->b, this->c, this->u, this->v, this->w);
}

double& CANON_POSITION::operator[](int ind)
{
    switch (ind)
    {
    case 0:
        return x;
    case 1:
        return y;
    case 2:
        return z;
    case 3:
        return a;
    case 4:
        return b;
    case 5:
        return c;
    case 6:
        return u;
    case 7:
        return v;
    case 8:
        return w;
    default:
        printf("noelement!\n");
        return noElement;
    }
}
